#include <stdio.h>
#include "ros/ros.h"
#include "controlcan.h"
#include <can_msgs/Frame.h>


class zlgcan
{    
public:
    DWORD devType,devIndex,channel,baud; 
    DWORD Open(DWORD _devIndex=0,DWORD _channel=0,DWORD _baud=500,DWORD _devType=4);
    DWORD Transmit(can_msgs::Frame msg);
    void  Receive(ros::Publisher &nh);
};
/*打开和初始化CAN卡*/
DWORD zlgcan::Open(DWORD _devIndex,DWORD _channel,DWORD _baud,DWORD _devType)
{
    VCI_INIT_CONFIG config;
    config.AccCode = 0;
    config.AccMask = 0xffffffff;
    config.Filter = 1;
    config.Mode = 0;

    devType=_devType;
    devIndex=_devIndex;
    channel=_channel;
    baud = _baud;

    //通信速率设置
    switch(baud)
    {
        case 250:config.Timing0 = 0x01;config.Timing1 = 0x1C;break;
        case 500:config.Timing0 = 0x00;config.Timing1 = 0x1C;break;
        default:break;
    }
    
    if(!VCI_OpenDevice(devType, devIndex, 0)) {
        printf("VCI_OpenDevice failed\n");
        return 1;
    }

    if(!VCI_ResetCAN(devType,devIndex,channel))
    {
        printf("VCI_ResetCAN(%d) failed\n", channel);
        return 2;
    }

    if (!VCI_InitCAN(devType, devIndex, channel, &config))
    {
        printf("VCI_InitCAN(%d) failed\n", channel);
        return 3;
    }

    if(!VCI_StartCAN(devType,devIndex,channel))
    {
        printf("VCI_StartCAN(%d) failed\n", channel);
        return 4;
    }     
    printf("VCI_StartCAN(%d) succeeded\n", channel);

    return 0;
}

DWORD zlgcan::Transmit(can_msgs::Frame msg)
{
    VCI_CAN_OBJ txBuff;
    txBuff.ID=msg.id;
    txBuff.ExternFlag=msg.is_extended;
    txBuff.DataLen=msg.dlc;
    for(int i=0;i<msg.dlc;i++)
    {
        txBuff.Data[i]=msg.data[i];
    }
    VCI_Transmit(devType,devIndex,channel,&txBuff,1);
}

void  zlgcan::Receive(ros::Publisher &publisher)
{
    _VCI_CAN_OBJ RxBuff[100]; //数据接收
    DWORD RxCount=0;
    DWORD temp=0;
    RxCount=VCI_GetReceiveNum(devType,devIndex,channel);
    if(RxCount!=0)
    {
        temp = RxCount;
        VCI_Receive(devType,devIndex,channel,RxBuff,RxCount,-1);
        while(RxCount)
        {
            //由RxBuff[0]开始发送 保证先收先发
            can_msgs::Frame msg;
            msg.id = RxBuff[temp-RxCount].ID;
            msg.is_extended = RxBuff[temp-RxCount].ExternFlag;
            msg.dlc = RxBuff[temp-RxCount].DataLen;
            for (uint8_t i=0; i < msg.dlc; i++)
            {
                msg.data[i] = RxBuff[temp-RxCount].Data[i]; 
            }
            publisher.publish(msg);
            RxCount--;
        }        
    }
}

zlgcan zlgcan;

void ZlgcanTxDataCallback(const can_msgs::Frame &msg)
{
    zlgcan.Transmit(msg);
}
int main(int argc, char **argv)
{
  //printf("请在启动程序时输入需要设置的CAN卡参数：如  ./zlgcan 0 1 500 表示启动CAN卡0 通道2 500kbps的通信数率");
  if(zlgcan.Open())
  {
      return 1;
  }
  ros::init(argc, argv, "Zlgcan");
  ros::NodeHandle n;

  ros::Publisher RxDataPublisher = n.advertise<can_msgs::Frame>("ZlgcanRxData", 1000);
  ros::Subscriber sub = n.subscribe("ZlgcanTxData",1000,ZlgcanTxDataCallback);

  //ros::Rate loop_rate(1000);//设置循环频率

  while (ros::ok())
  {
    zlgcan.Receive(RxDataPublisher);//读取接收的数据并发布到对应Topic
    ros::spinOnce();//必须调用否则无法执行回调函数
  //  loop_rate.sleep();
  }
  return 0;
}

